#include "extract.h"
#include <stdio.h>

using namespace cv;
using namespace std;
namespace enc = sensor_msgs::image_encodings;
using namespace cv;




class DetectEllipseNodeImpl : public DetectEllipseNode
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  ros::Subscriber caminfo_sub_;
  ros::Subscriber pointcloud2_sub_;
  ros::Publisher pointcloud2_pub_;

  image_transport::Subscriber image_sub_;
  //image_transport::Publisher image_pub_;
  image_geometry::PinholeCameraModel cam_model_;

  bool cam_bool;



public:
  DetectEllipseNodeImpl()
    : it_(nh_)
  {
    caminfo_sub_ = nh_.subscribe("camera_info", 1, &DetectEllipseNodeImpl::caminfoCb, this); 
    pointcloud2_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("point_cloud",1,&DetectEllipseNodeImpl::pointcloudCb,this);
    pointcloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("extract_points",1);
    image_sub_ = it_.subscribe("image", 1, &DetectEllipseNodeImpl::imageCb, this);

    cam_bool = false;
    // cv::namedWindow("src");
    // cv::namedWindow("dst");
    // cv::namedWindow("dst1");
    // cv::namedWindow("dst2");
    // cv::waitKey(10);
  }

  ~DetectEllipseNodeImpl(){

    cv::destroyWindow("src");
    cv::destroyWindow("dst");
    cv::destroyWindow("dst1");
    cv::destroyWindow("dst2");


  }

  virtual void caminfoCb(const sensor_msgs::CameraInfoConstPtr& msg)
  {
    cam_model_.fromCameraInfo(msg);
    cam_bool = true;
  }

  virtual void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    //encode msg image to CV::Mat
    cv_bridge::CvImagePtr cv_ptr;
    try
      {
	cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      }
    catch (cv_bridge::Exception& e)
      {


	ROS_ERROR("cv_bridge exception: %s", e.what());
	return;
      }
  }

  
  virtual void pointcloudCb(const sensor_msgs::PointCloud2ConstPtr &msg)
  {
    sensor_msgs::PointCloud2 out_msg;
    //pcl::PointCloud<pcl::PointXYZRGBNormal> points;
    pcl::PointCloud<pcl::PointXYZRGB> points;
    //pcl::PointCloud<pcl::PointXYZ> points;
    //pcl::PointCloud<pcl::PointXYZRGBNormal> normal_points;
    pcl::PointCloud<pcl::PointXYZRGB> normal_points;
    //pcl::PointCloud<pcl::PointXYZ> normal_points;
    pcl::fromROSMsg(*msg, points);
    normal_points.header = points.header;
    normal_points.width = points.width;
    normal_points.height = points.height;
    normal_points.is_dense = points.is_dense;
    //pcl::fromROSMsg(*msg, normal_points);
    for(size_t i = 0; i < points.points.size(); i++)
      {
	pcl::PointXYZRGB p;
	//pcl::PointXYZ p;
    	//pcl::PointXYZRGBNormal p;
    	p.x = points.points[i].x;
        p.y = points.points[i].y;
        p.z = points.points[i].z;
	// p.normal[0] = points.points[i].normal[0];
        // p.normal[1] = points.points[i].normal[1];
        // p.normal[2] = points.points[i].normal[2];
        p.rgb = points.points[i].rgb;
	//ROS_ERROR("(%f %f %f) ", p.x, p.y, p.z);
	// ROS_ERROR("(%f %f %f) ", points.points[i].x
	// 	  , points.points[i].y
	// 	  , points.points[i].z);
    	normal_points.points.push_back(p);
      }

    pcl::toROSMsg(normal_points, out_msg);

    pointcloud2_pub_.publish(out_msg);
  }
  
};

DetectEllipseNodePtr CreateDetectEllipseNode()
{
    return DetectEllipseNodePtr(new DetectEllipseNodeImpl());
}
